#include "MachineHelper.h"
#include <QDebug>

MachineHelper::MachineHelper():
    _mb_ptr{Singleton<mbControl>::get()}
{
    // run();
}


void MachineHelper::run()
{
    std::thread([this]()mutable{
        try
        {
            if(!_mb_ptr)
                throw ERR_DEVICE_MODBUS_DISCONNECTED;
            std::this_thread::sleep_for(std::chrono::seconds(5));
            int sig = 0;

            while(1)
            {
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
                try{
                    // _mb_ptr->recvMsg();
                    // sig = _mb_ptr->dealMsg();
                    if (sig)
                        cout<<"sig = "<<sig<<endl;
                    if(sig == -1)
                        continue;
                    if(sig > 0 && sig < 99)
                        undo();
                        // emit sig_operate(sig);
                    //_mb_ptr->sendMsg();
                }catch(...){
                    std::cout << BOLDRED << "PLC未找到寄存器!"<< RESET<<std::endl;
                }

            }
        }
        catch(...)
        {
            cout<<getErrString(ERR_FUNC_SERVICE_PROCESS_RUN_ERROR)<<endl;
        }
    }).detach();
}

MU_RETURNS MachineHelper::ABSMove(MachineArgs arg,bool safeonly)
{
    if(safeonly){
        _mb_ptr->rotateMotor(ROTATINGDISK,arg.Sr,abs(arg.rspeed));
        _mb_ptr->rotateMotor(FLIPDISK,arg.Sa,abs(arg.aspeed));
        _mb_ptr->rotateMotor(GROUNDRAIL,arg.Su,abs(arg.uspeed));
    }else{
        _mb_ptr->rotateMotor(ROTATINGDISK,arg.r,abs(arg.rspeed));
        _mb_ptr->rotateMotor(FLIPDISK,arg.a,abs(arg.aspeed));
        _mb_ptr->rotateMotor(GROUNDRAIL,arg.u,abs(arg.uspeed));
    }
    return MU_RETURN_SUCCESS;
}

MU_RETURNS MachineHelper::RelativeMove(MachineArgs arg)
{
    return MU_RETURN_SUCCESS;
}



MU_RETURNS MachineHelper::setPara(int radius, int speed)
{
    // todo
}


void MachineHelper::Connect()
{
    if(currentStatus.GetInit(MU_DEVICE_MACHINE)){     /// 如果已经初始化了
        qWarning()<<"变位机已经初始化成功!"<<__FUNCTION__;
        return ;
    }
    currentStatus.setInitType(MU_DEVICE_MACHINE,MU_TY_RUNNING);
    std::future<MU_RETURNS> result = std::async(std::launch::async,&MachineHelper::_open,this);


    if(MU_RETURN_SUCCESS != result.get()){
        currentStatus.initFailed(MU_DEVICE_MACHINE);
        std::cout << "变位机连接:" << MU_Get_Msg(result.get()) << ".\n";
        return ;
    }

    currentStatus.initSucess(MU_DEVICE_MACHINE);
}

MU_RETURNS MachineHelper::_open()
{
    return MU_RETURN_SUCCESS;
}
